#include "Math/Quaternion.h"
#include "Framework/Prerequisites.h"
#include "Math/Matrix3.h"

namespace tyro
{

	namespace Math
	{

		template < class T >
		const Quaternion<T> Quaternion<T>::ZERO(0,0,0,0);

		template < class T >
		const Quaternion<T> Quaternion<T>::IDENTITY(0,0,0,1);

		template < class T >
		void tyro::Math::Quaternion<T>::ToRotationMatrix( Matrix3<T>& mat ) const
		{
			T xV  = x+x;
			T yV  = y+y;
			T zV  = z+z;
			T wxV = xV*w;
			T wyV = yV*w;
			T wzV = zV*w;
			T xxV = xV*x;
			T xyV = yV*x;
			T xzV = zV*x;
			T yyV = yV*y;
			T yzV = zV*y;
			T zzV = zV*z;

			mat.m00 = 1.0f-(yyV+zzV);
			mat.m01 = xyV-wzV;
			mat.m02 = xzV+wyV;
			mat.m10 = xyV+wzV;
			mat.m11 = 1.0f-(xxV+zzV);
			mat.m12 = yzV-wxV;
			mat.m20 = xzV-wyV;
			mat.m21 = yzV+wxV;
			mat.m22 = 1.0f-(xxV+yyV);
		}




		template < class T >
		void tyro::Math::Quaternion<T>::ToAxis( Vector3<T>* axis ) const
		{
			Matrix3<T> rot;

			ToRotationMatrix(rot);

			for (size_t i = 0; i < 3; i++)
			{
				
				axis[i].x = rot[static_cast<size_t>(0)][i];
				axis[i].y = rot[static_cast<size_t>(1)][i];
				axis[i].z = rot[static_cast<size_t>(2)][i];
			}
		}


		

		template < class T >
		void tyro::Math::Quaternion<T>::FromRotationMatrix(const Matrix3<T>& rot)
		{
			// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
			// article "Quaternion Calculus and Fast Animation".

			T trace = rot[size_t(0)][size_t(0)] + rot[size_t(1)][size_t(1)] + rot[size_t(2)][size_t(2)];
			T root;

			if (trace > 0.0f)
			{

				root = Math::Sqrt(trace + 1.0f);  
				w = 0.5f * root;
				root = 0.5f / root;  
				x = (rot[size_t(2)][size_t(1)] - rot[size_t(1)][size_t(2)]) * root;
				y = (rot[size_t(0)][size_t(2)] - rot[size_t(2)][size_t(0)]) * root;
				z = (rot[size_t(1)][size_t(0)] - rot[size_t(0)][size_t(1)]) * root;

			} 
			else
			{
				static size_t s_iNext[3] = { 1, 2, 0 };
				size_t i = 0;
				if ( rot[size_t(1)][size_t(1)] > rot[size_t(0)][size_t(0)] )
					i = 1;
				if ( rot[size_t(2)][size_t(2)] > rot[size_t(i)][size_t(i)] )
					i = 2;
				size_t j = s_iNext[i];
				size_t k = s_iNext[j];

				root = Math::Sqrt(rot[size_t(i)][size_t(i)]-rot[size_t(j)][size_t(j)]-rot[size_t(k)][size_t(k)] + 1.0f);
				T* apkQuat[3] = { &x, &y, &z };
				*apkQuat[i] = 0.5f*root;
				root = 0.5f/root;
				w =				(rot[size_t(k)][size_t(j)]-rot[size_t(j)][size_t(k)])*root;
				*apkQuat[j] = (rot[size_t(j)][size_t(i)]+rot[size_t(i)][size_t(j)])*root;
				*apkQuat[k] = (rot[size_t(k)][size_t(i)]+rot[size_t(i)][size_t(k)])*root;

			}
		}


		template class Quaternion< tyro::FLOAT_32 >; 
		template class Quaternion< tyro::FLOAT_64 >; 


	}

}